#include "test_cam.h"
#include "ocr_procs.h"
#include "common.h"

int WIDTH 	= 640;
int HEIGHT 	= 480;

TRect rgbBuf;        // camera pix
TRect frBuf;        // frame buffer


struct {
                void *start;
                size_t length;
} *buffers;

struct v4l2_requestbuffers reqbuf;
struct v4l2_buffer buffer;


void scaleLine(PIXEL *Target, PIXEL *Source, int SrcWidth, int TgtWidth)
{
    int NumPixels = TgtWidth;
    int IntPart = SrcWidth / TgtWidth;
    int FractPart = SrcWidth % TgtWidth;
    int E = 0;

    while (NumPixels-- > 0) {
        *Target++ = *Source;
        Source += IntPart;
        E += FractPart;
        if (E >= TgtWidth) {
            E -= TgtWidth;
            Source++;
        } /* if */
    } /* while */
}

void scaleRect(PIXEL *Target, PIXEL *Source, int SrcWidth, int SrcHeight,
               int TgtWidth, int TgtHeight)
{
    int NumPixels = TgtHeight;
    int IntPart = SrcHeight/TgtHeight;
    IntPart *= SrcWidth;
    //printf("intPart = %d\n", IntPart);
    int FractPart = SrcHeight % TgtHeight;
    //printf("fracPart = %d\n", FractPart);
    int E = 0;
    PIXEL *PrevSource = NULL;

    while (NumPixels-- > 0) {
        if (Source == PrevSource) {
            memcpy(Target, Target-TgtWidth, TgtWidth*sizeof(*Target));
        } else {
            scaleLine(Target, Source, SrcWidth, TgtWidth);
            PrevSource = Source;
        } /* if */
        Target += TgtWidth;
        Source += IntPart;
        E += FractPart;
        if (E >= TgtHeight) {
            E -= TgtHeight;
            Source += SrcWidth;
        } /* if */
    } /* while */
}

int DrawRect(TRect SrcRect, TRect *DstRect) {
    scaleRect((PIXEL *)DstRect->addr, (PIXEL *)SrcRect.addr,
              SrcRect.width, SrcRect.height,
              DstRect->width, DstRect->height);

    return 0;
}


PIXEL** convertTo2D(TRect SrcRect){
    int i;
    PIXEL** pixels;
    int h = SrcRect.height;
    int w = SrcRect.width;
    printf("convertTo2d h = %d, w = %d, pixsize = %d\n", h, w, sizeof(PIXEL));
    pixels = (PIXEL**) malloc(h * sizeof(PIXEL*));
    for(i = 0; i < h; i++){
        pixels[i] = (PIXEL*) malloc(w * sizeof(PIXEL));
        memcpy(pixels[i], (SrcRect.addr + i*w), w*sizeof(PIXEL));
    }
    printf("endconvertTo2d\n");

    return pixels;
}

/**
 * Convert YUYV to RGB
 */
void yuyv2rgb(PIXEL *rgb, uint8_t *yuv, int width, int height){
        int i, j, yOff, rOff;
        uint8_t Y0, U, Y1, V, R0, G0, B0,
                                                R1, G1, B1;
        for(i = 0; i < width/2; i++) {
                for(j = 0; j < height; j++) { /// four bytes -> two pixels
                        yOff = j* 2*width + i*4;
                        rOff = j*width + i*2;

                        Y0 = yuv[yOff];
                        U = yuv[yOff + 1];
                        Y1 = yuv[yOff + 2];
                        V = yuv[yOff + 3];
                        /// pixel 1
                        B0 = CLIP(1.164*(Y0 - 16) + 2.018*(U - 128));
                        G0 = CLIP(1.164*(Y0 - 16) - 0.813*(V - 128) - 0.391*(U - 128));
                        R0 = CLIP(1.164*(Y0 - 16) + 1.596*(V - 128));
                        /// pixel 2
                        B1 = CLIP(1.164*(Y1 - 16) + 2.018*(U - 128));
                        G1 = CLIP(1.164*(Y1 - 16) - 0.813*(V - 128) - 0.391*(U - 128));
                        R1 = CLIP(1.164*(Y1 - 16) + 1.596*(V - 128));

                        /// save to rgb 565
                        rgb[rOff] = (B0 >> 3) | (((G0 >> 2) & 0x3f)  << 5 )| (((R0 >>3) & 0x1f) <<11);
                        rgb[rOff+1] = (B1 >> 3) | (((G1 >> 2) & 0x3f)  << 5 )| (((R1 >>3) & 0x1f) <<11);
                }
        }
}

void yuyv2gray(PIXEL * gray, uint8_t *yuv, int width, int height) {
        int i, j, yOff, rOff;
        uint8_t Y;

        for(i = 0; i < width; i++) {
                for(j = 0; j < height; j++) { /// four bytes -> two pixels
                        yOff = j*2*width + i*2;
                        rOff = j*width + i;

                        Y = yuv[yOff];
                        gray[rOff] = (Y >> 3) | ((Y >> 2) << 5) | ((Y >> 3) << 11);
                }
        }
}

/**
 * Init frame buffer
 */
void initFrameBuffer() {
        char *devName;
        int fd;

        frBuf.addr = (PIXEL*)MAP_FAILED;
        devName = "/dev/fb0";
        fd = open(devName, O_RDWR);
        if(fd < 0) perror("Can't open frame buffer\n");

        struct fb_fix_screeninfo Fix;
        struct fb_var_screeninfo Var;
        if (ioctl(fd, FBIOGET_FSCREENINFO, &Fix) < 0 || ioctl(fd, FBIOGET_VSCREENINFO, &Var) < 0) {
                perror("Can't get frame buffer information");
        }

        frBuf.BPP = Var.bits_per_pixel;
        frBuf.width  = Var.xres; 	//240
        frBuf.height = Var.yres;	//320

        frBuf.addr = (PIXEL *)mmap(0, frBuf.height * frBuf.width, PROT_READ|PROT_WRITE, MAP_SHARED, fd, 0);
        printf("start map\n");

        if (frBuf.addr == (PIXEL *)MAP_FAILED){
                perror("Map frame buffer failed");
                goto exit;
        }
        exit:
        close(fd);
}

void capture(int fd){
    int i, j;
    /** read from buffer */
    Image grayImg = createImage(HEIGHT, WIDTH);
    for(j = 0; j < HEIGHT; j++){
        for(i = 0; i < WIDTH;i++){
            grayImg.pixels[j][i] = (*(uint8_t *)(buffers[buffer.index].start+ 2*(i+j*WIDTH)));
        }
    }
    printf("read complete\n");
    ocrProcess(grayImg);


    /** Cleanup. */
    for (i = 0; i < reqbuf.count; i++)
    munmap (buffers[i].start, buffers[i].length);

    close(fd);
}

void preview(int fd){
    int ret = 0;

    /** get data by dequeue and enqueue */
    memset(&buffer, 0, sizeof(struct v4l2_buffer));
    buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buffer.memory = V4L2_MEMORY_MMAP;

    /// dequeue buffer
    ret = ioctl(fd, VIDIOC_DQBUF, &buffer);
    if (ret < 0)
    {
            perror("VIDIOC_DQBUF - Unable to dequeue buffer ");
            exit (EXIT_FAILURE);
    }

    /// enqueue buffer
    ret = ioctl(fd, VIDIOC_QBUF, &buffer);
    if (ret < 0)
    {
            perror("VIDIOC_QBUF - Unable to queue buffer");
            exit (EXIT_FAILURE);
    }

    /** preview data
    yuyv2gray((PIXEL*)rgbBuf.addr, buffers[buffer.index].start , WIDTH, HEIGHT);
    DrawRect(rgbBuf, &frBuf); */

}

int initCam()
{
    int dev_fd;
    struct v4l2_capability cap;
    struct v4l2_format fmt;

    /** Init frame buffer device
    initFrameBuffer();
    frBuf.width = 240;
    frBuf.height = 192;*/

    /** Camera process */
    if ((dev_fd = open( CAM_DEV, O_RDWR)) == -1)
            perror("ERROR opening camera interface\n");

    if (0 != ioctl(dev_fd, VIDIOC_QUERYCAP, &cap)) {
            if (EINVAL == errno) {
                    fprintf (stderr, "No V4L2 device\n");
                    return 1;
            } else {
                    perror ("VIDIOC_QUERYCAP");
                    return 1;
            }
    }
    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
            fprintf (stderr, "No video capture device\n");
            return 1;
    }

    /// set size of capture image
    memset(&fmt,sizeof(fmt),0);
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width = WIDTH;
    fmt.fmt.pix.height = HEIGHT;
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
    if(0 != ioctl(dev_fd, VIDIOC_S_FMT, &fmt)){
            perror("VIDIOC_S_FMT");
            return 1;
    }
    if(fmt.fmt.pix.width != WIDTH || fmt.fmt.pix.height != HEIGHT) {
            printf("mode %d x %d not support\n", WIDTH, HEIGHT);
            WIDTH = fmt.fmt.pix.width;
            HEIGHT = fmt.fmt.pix.height;
    }

    /** Buffer rgb */
    rgbBuf.width = WIDTH;
    rgbBuf.height = HEIGHT;
    rgbBuf.BPP = 16;
    rgbBuf.addr = malloc(WIDTH* HEIGHT * sizeof(PIXEL));

    int ret = 0;
    int i;

    /** set request buffer param */
    memset (&reqbuf, 0, sizeof (reqbuf));
    reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    reqbuf.memory = V4L2_MEMORY_MMAP;
    reqbuf.count = NB_BUFFER;
    /// request buffer
    if (-1 == ioctl (dev_fd, VIDIOC_REQBUFS, &reqbuf)) {
            if (errno == EINVAL)
                    printf ("Video capturing or mmap-streaming is not supported\n");
            else
                    perror ("VIDIOC_REQBUFS");
            exit (EXIT_FAILURE);
    }

    /** map buffer */
    buffers = calloc (reqbuf.count, sizeof (*buffers));
    assert (buffers != NULL);
    for (i = 0; i < reqbuf.count; i++) {
            memset (&buffer, 0, sizeof (struct v4l2_buffer));
            buffer.type = reqbuf.type;
            buffer.memory = V4L2_MEMORY_MMAP;
            buffer.index = i;
            if (-1 == ioctl (dev_fd, VIDIOC_QUERYBUF, &buffer)) {
                    perror ("VIDIOC_QUERYBUF");
                    exit (EXIT_FAILURE);
            }
            buffers[i].length = buffer.length; /* remember for munmap() */
            buffers[i].start = mmap (NULL, buffer.length,
            PROT_READ | PROT_WRITE, /* recommended */
            MAP_SHARED,
            /* recommended */
            dev_fd, buffer.m.offset);
            if (MAP_FAILED == buffers[i].start) {
                    /* If you do not exit here you should unmap() and free()
                    the buffers mapped so far. */
                    perror ("mmap");
                    exit (EXIT_FAILURE);
            }
    }

    /** queue buffer */
    for (i = 0; i < reqbuf.count; ++i)
    {
            memset(&buffer, 0, sizeof(struct v4l2_buffer));
            buffer.index = i;
            buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
            buffer.memory = V4L2_MEMORY_MMAP;
            ret = ioctl(dev_fd, VIDIOC_QBUF, &buffer);
            if (ret < 0)
            {
                    perror("VIDIOC_QBUF - Unable to queue buffer");
                    exit (EXIT_FAILURE);
            }
    }

    /** enable video stream */
    int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    ret = ioctl(dev_fd, VIDIOC_STREAMON, &type);
    if (ret < 0)
    {
            perror("VIDIOC_STREAMON - Unable to start capture");
            exit (EXIT_FAILURE);
    }
    return dev_fd;
}
